#include "zf_common_headfile.h"

// 计算方向误差的PID
float pid_solve_nom (pid_param_t *pid, float error)
{
    float low_pass = 0.6;
    pid->out_p = error;
    pid->out_i += error;
    pid->out_d = (error - pid->out_p) * low_pass + pid->out_d * (1 - low_pass);

   if (pid->ki != 0)
       pid->out_i = MINMAX(pid->out_i, -30, 30);

    return pid->kp * pid->out_p + pid->ki * pid->out_i + pid->kd * pid->out_d;
}

// 计算方向误差的PD
float pid_solve (pid_param_t *pid, float error)
{
    float low_pass = 0.6;
    pid->out_p = error;
    pid->out_d = (error - pid->out_p) * low_pass + pid->out_d * (1 - low_pass);

    return pid->kp * pid->out_p + pid->kd * pid->out_d;
}

// 计算电机占空比PID
float increment_pid_solve (pid_param_t *pid, motor_param *motor)
{
    float error = (float) (motor->target_speed - motor->current_speed);
    pid->out_p = MINMAX(pid->kp * (error - pid->pre_error), -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX);
    pid->out_i = MINMAX(pid->ki * error, -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX);
    pid->out_d = MINMAX(pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error), -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX);
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return (pid->out_p + pid->out_i + pid->out_d);
}

float change_kib = 4;

//鍙樼Н鍒哖ID锛宔澶灏�
float changable_pid_solve (pid_param_t *pid,motor_param *motor)
{
    float error;

        error = (float) (motor->target_speed - motor->current_speed);

    pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);
    pid->out_p = pid->kp * (error - pid->pre_error);
    float ki_index = pid->ki;
    if (error + pid->pre_error > 0)
    {
        ki_index = (pid->ki) - (pid->ki) / (1 + exp(change_kib - 0.2 * fabs(error)));    //鍙樼Н鍒嗘帶鍒�
    }

    pid->out_i = ki_index * error;
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return MINMAX(pid->out_p, -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX) + MINMAX(pid->out_i, -MOTOR_PWM_DUTY_MAX,MOTOR_PWM_DUTY_MAX)
            + MINMAX(pid->out_d, -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX);
}


float bangbang_out = 0;

float bangbang_pid_solve (pid_param_t *pid, motor_param *motor)
{
    float BangBang_output = 10000, BangBang_error = 20;

    float error;

    error = (float) (motor->target_speed - motor->current_speed);

    pid->error = error;

    //BangBang
    if (error > BangBang_error || error < -BangBang_error)
    {
        bangbang_out = (error > 0) ? BangBang_output : (-BangBang_output);
    }
    else
    {
        pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);

        pid->out_p = pid->kp * (error - pid->pre_error);

        pid->out_i = pid->ki * error;

        bangbang_out = MINMAX(pid->out_p, -MOTOR_PWM_DUTY_MAX,MOTOR_PWM_DUTY_MAX) + MINMAX(pid->out_i, -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX)
        + MINMAX(pid->out_d, -MOTOR_PWM_DUTY_MAX,MOTOR_PWM_DUTY_MAX);

    }
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return bangbang_out;
}
